package physgin;

import java.util.Calendar;
import javax.vecmath.Point3f;
import javax.vecmath.Vector2f;

/**
 * Controls the physics of an individual Circle object
 * Used to calculate position and velocity during
 * normal movement and collisions.
 * Contains all of the physical variables for the object.
 * @see     java.lang.Cloneable
 */
class CirclePhysics implements Physics, Cloneable
{
    private float Cr;
    private float mass;
    private float size;
    private float orientation; //radians
    private float volume;
    private float density;
    private float xArea;
    private float Cd; //drag coefficiant
    
    private Point3f position;
    private Point3f velocity;
    private Point3f accel;
    private Point3f force;
    
    private Shapes shape;
    private long oldTime;
    private float currentTime;
    private Calendar cal;
    
    /**
     * Default Class Constructor.
     *
     * @param   e   float of the Coefficient of Restitution of the object (Bounciness)
     * @param   m   float of the mass of the object
     * @param   s   float of the Size of the object (Radius in Circle)
     * @param   o   float of the Orientation of the object, in radians 
     * @param   p   Point3f representing XYZ position
     * @param   v   Point3f representing XYZ velocity
     */
    CirclePhysics(float e, float m, float s, float o, Point3f p, Point3f v) 
    {
        shape = Shapes.CIRCLE;
        Cr = e;
        mass = m;
        size = s;
        orientation = o;
        volume = (4f/3f) * (float) Math.PI * (s*s*s); 
        density = mass / volume;
        xArea = (float) Math.PI * (s*s);
        Cd = 0.2f;
        
        position = new Point3f(p);
        velocity = new Point3f(v);
        accel = new Point3f(0f,0f,0f);
        accel.add(Physics.grav);
        force = new Point3f(mass*accel.x, mass*accel.y, mass*accel.z);
        
        cal = Calendar.getInstance();
        oldTime = cal.getTimeInMillis();
    }
    
    /**
     * A Minimal constructor
     */
    CirclePhysics(Point3f pos, float m, float s)
    {
        this(0.8f, m, s, 0f, pos, new Point3f(0f,0f,0f));
    }
    
    /**
     * Function to clone a physics object for collision calculations.
     *
     * @return      Clone of this object as an Object.
     */
    protected Object clone()
    {
        try {
            return super.clone();
        }
        catch (CloneNotSupportedException e) {
            // This should never happen
            throw new InternalError(e.toString());
        }
    }
    
    /**
     * Call to the object to return the private Point3f Position.
     *
     * @return    Point3f position.
     */
    public Point3f getPos()
    {  
        //updatePos();
        return position; //new Point3f(position);  
    }
    
    /**
     * Call to the object to return the private float Mass.
     *
     * @return   float mass
     */
    public float getMass()
    {  
        return mass;  
    }
    
    /**
     * Call to the object to return the private Point3f Velocity.
     *
     * @return   Point3f Velocity
     */
    public Point3f getVel()
    {  
        return velocity; //new Point3f(velocity);  
    }
    
    /**
     * Call to the object to return the private float Size.
     *
     * @return   float size
     */
    public float getSize()
    {  
        return size; 
    }
    
    /**
     * Call to the object to return the private float Orientation.
     *
     * @return  float orientation
     */
    public float getOrient()
    {  
        return orientation;
    }
    
    /**
     * Call to the object to return the private enum Shape.
     *
     * @return    Shapes shape
     */
    public Shapes getShape()
    {
      return shape;
    }
    
    /**
     * Calculates the forces on the object using f = m*a.
     *
     * @return    Point3f f
     */
    public Point3f getForce()
    {
        cal = Calendar.getInstance();
        long a = cal.getTimeInMillis();
        currentTime = a - oldTime;
        
        Point3f f;
        float x;
        float y;
        float d;
        
        d = accel.x*currentTime;
        x = (d * mass)/currentTime;
        
        d = accel.y*currentTime;
        y = (d * mass)/currentTime;
        
        f = new Point3f(x, y, 0.0f);
        return f;
    }

    /**
     * Call to the object to return the physics object as a clone
     *
     * @return    Physics object
     */
    public Physics getPhysics()
    {
        return (Physics) this.clone();
    }
    
    
    /**
     * Call to the object to update its position based on
     * velocity and acceleration.
     *
     * @return    void
     */
    public void updatePos()
    {
        cal = Calendar.getInstance();
        long a = cal.getTimeInMillis();
        currentTime = a - oldTime;
        
        float velx = velocity.x;
        float vely = velocity.y;
        float posx = position.x;
        float posy = position.y;
        float accelx = accel.x;
        float accely = accel.y;
        float dx, dy;
        
        int d = 1000;
        currentTime /= d;
        
        // at
        float xat = (currentTime * accelx); 
        float yat = (currentTime * accely);
        
        if(Math.abs(vely) < 0.05)
          vely = 0;
        if(Math.abs(velx) < 0.05)
          velx = 0;
        
        // s = s0 + ut + 1/2(at^2)
        posx = posx + (velx*currentTime) + (0.5f*xat*currentTime);
        posy = posy + (vely*currentTime) + (0.5f*yat*currentTime);
        
        //Continuous naive rotation
        orientation += (velx*currentTime)*(float)Math.PI;

        //drag
        dx = -1f * 1f/2f * density * Math.abs(velx) * xArea * Cd; //(velx*velx)
        dy = -1f * 1f/2f * density * Math.abs(vely) * xArea * Cd; //(vely*vely)
        
        //From F=ma=(dv*m)/dt :. dv = dt*F/m
        velx += (velx<0?-1:1)*(currentTime*(force.x+dx))/mass;
        vely += (currentTime*(force.y+dy))/mass;
        
        if (posx == Float.NaN)
            posx = 0.0f + size;
        if (posy == Float.NaN)
            posy = 0.0f + size;
        
        position = new Point3f(posx, posy, position.z);
        velocity = new Point3f(velx, vely, velocity.z);
        oldTime = a;
    }
    
    /**
     * Call to the object to update its position and velocity
     * based on a collision. Uses the colliding physics object
     * for collision information.
     *
     * @param   pB  Physics object with which to calculate how to update this one given a collision between the two.
     * @return    void
     */
    public void resolve(Physics pB)
    {
      //Get Positions,Velocities and mass
      Vector2f Apos = new Vector2f(this.getPos().x, this.getPos().y);
      Vector2f Bpos = new Vector2f(pB.getPos().x, pB.getPos().y);
      Vector2f Avel = new Vector2f(this.getVel().x, this.getVel().y);
      Vector2f Bvel = new Vector2f(pB.getVel().x, pB.getVel().y);
      float Amass = this.getMass();
      float Bmass = pB.getMass();
      
      // COLLISION CODE - SQUARE Horizontal/Vertical collisions
      if(pB.getShape() == Shapes.SQUARE)
      {
        if(Math.abs(Apos.x - Bpos.x) < Math.abs(Apos.y - Bpos.y)) //Circle is above/below square
        {
          float vely = (((Cr + 1)*Bmass*Bvel.y) + (Avel.y*(Amass - (Cr* Bmass)))) / (Amass + Bmass);
          if(Bmass > 100f) // MASSIVE OBJECT ie. walls which dont move.
            vely = Cr*Avel.y*-1.0f;
          velocity = new Point3f(Avel.x,vely,velocity.z);
          if(Apos.y > Bpos.y) //ensure object does not penetrate by repositioning in y.
            position = new Point3f(Apos.x,(Bpos.y +(pB.getSize()+ 0.001f +this.size)),position.z);
          else
            position = new Point3f(Apos.x,(Bpos.y- (pB.getSize() +this.size + 0.001f)),position.z);
          return;
        }
        if(Math.abs(Apos.x - Bpos.x) > Math.abs(Apos.y - Bpos.y)) //Circle is horizonatal to square
        {
          float velx = (((Cr + 1)*Bmass*Bvel.x) + (Avel.x*(Amass - (Cr* Bmass)))) / (Amass + Bmass);
          if(Bmass > 100f)
            velx = Cr*Avel.x*-1.0f;
          velocity = new Point3f(velx,Avel.y,velocity.z);
          if(Apos.x > Bpos.x)
            position = new Point3f((Bpos.x + (pB.getSize() + 0.001f +this.size)),Apos.y,position.z);
          else
            position = new Point3f((Bpos.x- (pB.getSize() + this.size + 0.001f)),Apos.y,position.z);
          return;
        }
      }
      /* COLLISION CODE - CIRCLES and CORNER SQUARE */
      
      Vector2f UNIT = new Vector2f(Bpos.x - Apos.x,Bpos.y - Apos.y);
      UNIT.normalize(); // unit normal vector
      Vector2f TANGENT = new Vector2f(-UNIT.y, UNIT.x); // unit tangent vector
	
      // project velocities onto the axes using the dot product
      float Aunit = UNIT.dot(Avel);
      float Atan = TANGENT.dot(Avel);
      float Bunit = UNIT.dot(Bvel);
      float Btan = TANGENT.dot(Bvel);
	
      // calculate normal velocities using restitution equation in 1d
      float Anorm = (((Cr + 1)*Bmass*Bunit) + (Aunit*(Amass - (Cr* Bmass)))) / (Amass + Bmass);
  
      // new normal and tangential velocity vectors
      Vector2f AnormV = new Vector2f((Anorm * UNIT.x),(Anorm *UNIT.y));
      Vector2f AtanV = new Vector2f((Atan * TANGENT.x),(Atan * TANGENT.y));      
      
      // Set new velocities in x and y coordinates
      float newx = (AnormV.x + AtanV.x);
      float newy = (AnormV.y + AtanV.y);
      velocity = new Point3f(newx,newy,0);
      
      /* COLLISION END - REPOSITION START */
      
      float coly, colx;
      
      //Normal Circle to Circle resolution.

        colx = (Apos.x + Bpos.x)/2.0f; //Collision Point
        coly = (Apos.y + Bpos.y)/2.0f;

      float diffax = (colx) - Apos.x; // Distance from collision
      float diffay = (coly) - Apos.y;
      float afromc = (float)Math.sqrt(Math.pow((diffax),2.0f) + Math.pow((diffay),2.0f));
      float comparea = (getSize() / afromc );
      position = new Point3f(colx - (diffax * comparea), coly - (diffay * comparea), this.getPos().z);
      /* REPOSITION END */
    }
}
